#include <math.h>
#include "Ballistic2D.h"

#define G_ACCEL 9.8
#define PI 3.141592653

#define FLAG_SPEED 1
#define FLAG_ANGLE 2
#define FLAG_TARGETX 4
#define FLAG_TARGETY 8

//#define DEBUG 1
//#define UNIT_TEST 1

#ifdef DEBUG
#include <iostream>
using namespace std;
#endif /* DEBUG */

void Ballistic2D::SetSpeed(float speed){
	 intialspeed = speed;
	 set = set | FLAG_SPEED;
 }

void Ballistic2D::SetAngle(float ang){
	angle = ang;
	set = set | FLAG_ANGLE;
}

void Ballistic2D::SetTargetX(float x){
	targetx = x;
	set = set| FLAG_TARGETX;
}

void Ballistic2D::SetTargetY(float y){
	targety = y;
	set = set|FLAG_TARGETY;
}


int Ballistic2D::FindTargetX(float* x1, float* x2){
	int rc = B2D_SUCCESS;
	if ((set & (FLAG_SPEED | FLAG_ANGLE | FLAG_TARGETY)) ==
			(FLAG_SPEED | FLAG_ANGLE | FLAG_TARGETY)) {
		float distancey = targety - yposition; // gets the right distance from the ballistic

		float voy = intialspeed*sin((angle*PI)/180);
#ifdef DEBUG
		cout << "voy: " << voy << endl;
#endif

		float disc = (voy*voy)-2*G_ACCEL*distancey;
#ifdef DEBUG
		cout << "disc: " << disc << endl;
#endif

		if(disc >= 0){
			float time1;
			float time2;
			time1 = (voy-sqrtf(disc))/G_ACCEL;
			time2 = (voy+sqrtf(disc))/G_ACCEL;
#ifdef DEBUG
			cout << "time1: " << time1 << endl;
			cout << "time2: " << time2 << endl;
#endif
			float vox = intialspeed*cos((angle*PI)/180);
#ifdef DEBUG
			cout << "vox: " << vox << endl;
#endif

			if (time1 < 0) {
				rc = B2D_SUCCESS_1RESULT;
				*x1 = vox*time2;
			}
			else{
				*x1 = vox*time1;
				*x2 = vox*time2;
			}
		}
		else {
			rc = B2D_ERR_UNREACHABLE;
		}
	}
	else {
		rc = B2D_ERR_INVALIDARG;
	}
	return rc;
}

int Ballistic2D::FindTargetY(float* y){
	int rc = B2D_SUCCESS;
	if ((set & (FLAG_SPEED | FLAG_ANGLE | FLAG_TARGETX)) ==
			(FLAG_SPEED | FLAG_ANGLE | FLAG_TARGETX)){
		float xdistance;
		xdistance = targetx - xposition;
		float vox;
		vox = intialspeed*cos((angle*PI)/180);
#ifdef DEBUG
		cout << "vox: " << vox << endl;
#endif
		float time;
		time = xdistance/vox;
#ifdef DEBUG
		cout << "time: " << time << endl;
#endif
		float voy;
		voy= intialspeed*sin((angle*PI)/180);
#ifdef DEBUG
		cout << "voy: " << voy << endl;
#endif
		*y = voy*time-0.5*G_ACCEL*time*time;

	}
	else{
		rc = B2D_ERR_INVALIDARG;
	}
		return rc;

}

int Ballistic2D::FindSpeed(float* speed){
	int rc = B2D_SUCCESS;
	if((set & (FLAG_ANGLE| FLAG_TARGETX | FLAG_TARGETY ))==
			(FLAG_ANGLE| FLAG_TARGETX |FLAG_TARGETY)){
		float rangle;
		rangle = (angle*PI)/180;
		float xdistance;
		float ydistance;
		xdistance = targetx - xposition;
		ydistance = targety - yposition;
		float det;
		det = xdistance*tan(rangle)-ydistance;
		if(det <= 0){
			rc = B2D_ERR_UNREACHABLE;
		}
		else{
			*speed = sqrt((G_ACCEL*xdistance*xdistance)/(2*det*cos(rangle)*cos(rangle)));
		}
	}
	else{
		rc = B2D_ERR_INVALIDARG;
	}
	return rc;
}

int Ballistic2D::FindAngle(float* angle){
	int rc = B2D_ERR_UNIMPLEMENTED;
	return rc;
}

#ifdef UNIT_TEST
#include <iostream>
using namespace std;

void TestFindTargetX(float speed,
					 float angle,
					 float y)
{
	Ballistic2D b;

	b.SetSpeed(speed);
	b.SetAngle(angle);
	b.SetTargetY(y);

	float x1, x2;
	int rc = b.FindTargetX(&x1, &x2);

	cout << "Speed: " << speed << endl;
	cout << "Angle: " << angle << endl;
	cout << "Target Y: " << y << endl;
	if (rc == B2D_SUCCESS) {
		cout << "Target X1: " << x1 << endl;
		cout << "Target X2: " << x2 << endl;
	}
	else {
		if (rc == B2D_SUCCESS_1RESULT){
			cout << "Target X1: " << x1 << endl;
		}
		else{
			cout << "Error code: " << rc << endl;
		}
	}
	cout << endl;
}

void TestFindTargetY(float speed,
					 float angle,
					 float x)
{
	Ballistic2D b;

	b.SetSpeed(speed);
	b.SetAngle(angle);
	b.SetTargetX(x);

	float y;
	int rc = b.FindTargetY(&y);

	cout << "Speed: " << speed << endl;
	cout << "Angle: " << angle << endl;
	cout << "Target X: " << x << endl;

	if(rc == B2D_SUCCESS){
		cout << "Target Y: " << y << endl;
	}
	else{
		cout << "Error Code: " << rc << endl;
	}
	cout << endl;
}

void TestFindSpeed(float angle,
					float x,
					float y)
{
	Ballistic2D b;

	b.SetAngle(angle);
	b.SetTargetX(x);
	b.SetTargetY(y);

	float speed;
	int rc = b.FindSpeed(&speed);

	cout << "angle: " << angle << endl;
	cout << "target X: " << x << endl;
	cout << "target Y: " << y << endl;

	if(rc == B2D_SUCCESS){
		cout << "Speed: " << speed << endl;
	}
	else{
		cout << "Error Code: " << rc << endl;
	}
	cout << endl;
}

int main (void)
{
	// Should produce two answers
	TestFindTargetX(100, 45, 20);
	// Should produce unreachable
	TestFindTargetX(1, 45, 35);
	// Should produce one answer
	TestFindTargetX(10, 45, -10);
	// believe it or not still two answers. Makes sense beacause PI is
	// not exact. We have tiny horizontal velocity
	TestFindTargetX(100, 90, 20);


	//Should give one answer
	TestFindTargetY(100, 45, 10);
	//try another speed
	TestFindTargetY(10, 45, 10);
	//Should give negative answer
	TestFindTargetY(1, 45, 10);
	//try diffrent speed
	TestFindTargetY(100, 30, 10);

	// Some of previous examples in reverse
	// Should return around 100
	TestFindSpeed(45, 10, 9.902);
	// Should return around 10
	TestFindSpeed(45, 10, 0.2);
	//Should return 100
	TestFindSpeed(30, 10, 5.70817);

	return 0;
}
#endif /* UNIT_TEST */
